#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<iostream>
#include "std_msgs/Int8.h"
#include <std_msgs/Float64.h>
#include <sensor_msgs/Joy.h>
#include <std_msgs/Float64MultiArray.h>
float WHEEL_L=2.8;
float WHEEL_R=0.6;
float WHEEL_PI=3.141693;
float v1=0,v2=0,v3=0,v4=0,v5=0,v6=0;
float cv1=0,cv2=0,cv3=0,cv4=0,cv5=0,cv6=0;
int joyFlag;


void joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{

  float speed_x,speed_y,speed_w;
  geometry_msgs::Twist twist;
  double l_scale_=2, a_scale_=2; 

  // take the data from the joystick and manipulate it by scaling it and using independent axes to control the linear and angular velocities of the turtle
if(joy->buttons[2])		//X键
{
	joyFlag=1;
	twist.angular.z = a_scale_*joy->axes[0];
	twist.linear.x = l_scale_*joy->axes[1];

	ROS_INFO("Received a /joy message!");
	speed_x = twist.angular.z;//v
	speed_y = 0;
	speed_w = twist.linear.x;//w

 	v1 =speed_x+0.5*WHEEL_L*speed_w;   //左边    //转化为每个轮子的线速度
 	v3 =v1;
 	v5 =v1;
   
 	v2 =-(speed_x-0.5*WHEEL_L*speed_w);
 	v4 =v2;
 	v6 =v2;
  
 	v1 =v1/(2.0*WHEEL_R*WHEEL_PI)*10;    //转换为轮子的速度　RPM
  	v2 =v2/(2.0*WHEEL_R*WHEEL_PI)*10;
  	v3 =v3/(2.0*WHEEL_R*WHEEL_PI)*10;
  	v4 =v4/(2.0*WHEEL_R*WHEEL_PI)*10;
 	v5 =v5/(2.0*WHEEL_R*WHEEL_PI)*10;
 	v6 =v6/(2.0*WHEEL_R*WHEEL_PI)*10;

}
else if(joy->buttons[0])
{
	joyFlag=0;
ROS_INFO("justice joyFlag=0");
} 

}


void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{

  float speed_x,speed_y,speed_w;
  

  ROS_INFO("Received a /cmd_vel message!");
  speed_x = msg->angular.z;//v
  speed_y = 0;
  speed_w = msg->linear.x;//w
  

   cv1 =speed_x+0.5*WHEEL_L*speed_w;   //左边    //转化为每个轮子的线速度
   cv3 =cv1;
   cv5 =cv1;
   
   cv2 =-(speed_x-0.5*WHEEL_L*speed_w);
   cv4 =cv2;
   cv6 =cv2;
  
   cv1 =cv1/(2.0*WHEEL_R*WHEEL_PI)*10;    //转换为轮子的速度　RPM
   cv2 =cv2/(2.0*WHEEL_R*WHEEL_PI)*10;
   cv3 =cv3/(2.0*WHEEL_R*WHEEL_PI)*10;
   cv4 =cv4/(2.0*WHEEL_R*WHEEL_PI)*10;
   cv5 =cv5/(2.0*WHEEL_R*WHEEL_PI)*10;
   cv6 =cv6/(2.0*WHEEL_R*WHEEL_PI)*10;
  

  
 

}
 
int main(int argc, char** argv)
{
	ros::init(argc, argv, "motive_control");
	ros::NodeHandle n;
	ros::Subscriber tsub = n.subscribe("/joy", 1000, joyCallback);
        ros::Subscriber sub = n.subscribe("/cmd_vel", 1000, cmd_vel_callback);
	ros::Publisher six_wheel_torque_pub = n.advertise<std_msgs::Float64MultiArray>("wheel_torque_publisher", 1000);//发布话题wheel_speed_publisher
        //ros::Publisher left_pub_1 = n.advertise<std_msgs::Int8>("MotionPubLeft1", 1000);
	//ros::Publisher right_pub_1 = n.advertise<std_msgs::Int8>("MotionPubRight1", 1000);
        //ros::Publisher left_pub_2 = n.advertise<std_msgs::Int8>("MotionPubLeft2", 1000);
	//ros::Publisher right_pub_2 = n.advertise<std_msgs::Int8>("MotionPubRight2", 1000);
        //ros::Publisher left_pub_3 = n.advertise<std_msgs::Int8>("MotionPubLeft3", 1000);
	//ros::Publisher right_pub_3 = n.advertise<std_msgs::Int8>("MotionPubRight3", 1000);
 	ros::Rate loop_rate(10);

while (ros::ok())
  {
    std_msgs::Float64MultiArray msg;
    //std_msgs::Int8 msg1;
    //std_msgs::Int8 msg2;
    //msg1.data = v1;
    //msg2.data = v2;
    //cmsg1.data = cv1;
    //cmsg2.data = cv2;
    //ROS_INFO("%d", msg1.data); //ROS_INFO("%f", msg.data.c_str())
    //ROS_INFO("%d", msg2.data);
if(joyFlag==1)
{
msg.data.push_back(v1);
msg.data.push_back(v2);
msg.data.push_back(v3);
msg.data.push_back(v4);
msg.data.push_back(v5);
msg.data.push_back(v6);

    //msg1.data = v1;
    //msg2.data = v2;
    ROS_INFO("joyFlag=1");
    //left_pub.publish(msg1);
    //right_pub.publish(msg2);
}
if(joyFlag==0)
{
msg.data.push_back(cv1);
msg.data.push_back(cv2);
msg.data.push_back(cv3);
msg.data.push_back(cv4);
msg.data.push_back(cv5);
msg.data.push_back(cv6);


    //msg1.data = cv1;
    //msg2.data = cv2;
    ROS_INFO("joyFlag=0");

}
six_wheel_torque_pub.publish(msg);
//left_pub_1.publish(msg1);
    //right_pub_1.publish(msg2);
//left_pub_2.publish(msg1);
    //right_pub_2.publish(msg2);
//left_pub_3.publish(msg1);
    //right_pub_3.publish(msg2);
    ros::spinOnce();
    loop_rate.sleep();
  }


  return 0;
}

